{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "3e1f10e7",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 4: Mobile Robot Vehicles\n",
    "\n",
    "Copyright (c) 2021- Peter Corke"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a582779",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    from google.colab import output\n",
    "    print('Running on CoLab')\n",
    "    #!pip install roboticstoolbox-python>=1.0.2\n",
    "    !pip install git+https://github.com/petercorke/robotics-toolbox-python@future\n",
    "    !pip install spatialmath-python>=1.1.5\n",
    "    !pip install --no-deps rvc3python\n",
    "    !pip install bdsim\n",
    "    COLAB = True\n",
    "except ModuleNotFoundError:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.display import HTML\n",
    "\n",
    "%matplotlib inline\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# add RTB examples folder to the path\n",
    "import sys, os.path\n",
    "import RVC3 as rvc\n",
    "sys.path.append(os.path.join(rvc.__path__[0], 'models'))\n",
    "\n",
    "# helper function to run bdsim in a subprocess and transfer results using a pickle file\n",
    "import pickle\n",
    "def run_shell(tool, **params):\n",
    "    global out\n",
    "    pyfile = os.path.join(rvc.__path__[0], \"models\", tool+\".py\")\n",
    "    cmd = f\"python {pyfile} -H +a -o\"\n",
    "    for key, value in params.items():\n",
    "        cmd += f' --global \"{key}={value}\"'\n",
    "    print(cmd)\n",
    "    os.system(cmd)\n",
    "    with open(\"bd.out\", \"rb\") as f:\n",
    "        out = pickle.load(f)\n",
    "\n",
    "# ------ standard imports ------ #\n",
    "import numpy as np\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from spatialmath import *\n",
    "from spatialmath.base import *\n",
    "from roboticstoolbox import *\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "af64cfca",
   "metadata": {},
   "source": [
    "# 4.1 Wheeled Mobile Robots\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "25080937",
   "metadata": {},
   "source": [
    "## 4.1.1 Car-Like Mobile Robots\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "d7bec320",
   "metadata": {},
   "source": [
    "<div style=\"background-color:red\">\n",
    "<span style=\"background-color:red; font-size:20pt\">NOTE</span>\n",
    "\n",
    "There are issues in running bdsim from inside Jupyter.  We have two options, disable all graphics (which is a bit dull) or run bdsim from a subprocess with its own popout windows.\n",
    "\n",
    "For Colab we can only do the first option.  \n",
    "\n",
    "Otherwise, we use the wrapper function `run_shell()` defined in the first cell to spawn a new Python instance to run the block diagram which will pop up new windows on your screen to display a simple animation showing the vehicle's motion in the plane.  This makes it a bit harder to get parameters and data between Jupyter and the `bdsim` simulation, so:\n",
    "* parameter values are passed to `bdsim` as `--global` command line options.\n",
    "* the `-o` command line option to `bdsim` causes it to write results as a pickle file, which are then imported back into Jupyter.</div>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5886e195",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m lanechange -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"lanechange\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d7ad8f79",
   "metadata": {},
   "outputs": [],
   "source": [
    "out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "30f08af1",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(out.t, out.x);  # q vs time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9139a140",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(out.x[:,0], out.x[:,1]);  # x vs y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e2baecea",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.close(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "48b3d3b3",
   "metadata": {},
   "source": [
    "### 4.1.1.1 Driving to a Point\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "05b194b0",
   "metadata": {},
   "outputs": [],
   "source": [
    "pgoal = (5, 5);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "abeb9a3e",
   "metadata": {},
   "outputs": [],
   "source": [
    "qs = (8, 5, pi / 2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b597c630",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "    %run -i -m drivepoint -H -g\n",
    "else:\n",
    "    run_shell(\"drivepoint\", pgoal=pgoal, qs=qs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50b6b0fc",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = out.x;  # configuration vs time\n",
    "plt.plot(q[:, 0], q[:, 1]);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5c61de3c",
   "metadata": {},
   "source": [
    "### 4.1.1.2 Driving Along a Line\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3b2edc8f",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = (1, -2, 4);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d1c7026",
   "metadata": {},
   "outputs": [],
   "source": [
    "qs = (8, 5, pi / 2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d78c8b7a",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m driveline -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"driveline\", L=L, qs=qs)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "589a8ae8",
   "metadata": {},
   "source": [
    "### 4.1.1.3 Driving Along a Path\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "248d3a48",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m drivepursuit -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"drivepursuit\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "db930268",
   "metadata": {},
   "source": [
    "### 4.1.1.4 Driving to a Configuration\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "645bd8d8",
   "metadata": {},
   "outputs": [],
   "source": [
    "qg = (5, 5, pi / 2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fd24ad73",
   "metadata": {},
   "outputs": [],
   "source": [
    "qs = (9, 5, 0);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b6280e6a",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m driveconfig -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"driveconfig\", qg=qg, qs=qs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2100965c",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = out.x;  # configuration vs time\n",
    "plt.plot(q[:, 0], q[:, 1]);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "13d977e3",
   "metadata": {},
   "source": [
    "# 4.2 Aerial Robots\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d5e36a5",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m quadrotor -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"quadrotor\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6e1d68fb",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = out.t; x = out.x;\n",
    "x.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f5248f38",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(t, x[:, 0], t, x[:, 1]);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "59695af5",
   "metadata": {},
   "source": [
    "# 4.4 Wrapping Up\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7b99c4de",
   "metadata": {},
   "source": [
    "## 4.4.1 Further Reading\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8b05c4f9",
   "metadata": {},
   "outputs": [],
   "source": [
    "veh = Bicycle(speed_max=1, steer_max=np.deg2rad(30));\n",
    "veh.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7b4714ac",
   "metadata": {},
   "outputs": [],
   "source": [
    "veh.step([0.3, 0.2])\n",
    "veh.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "be917c84",
   "metadata": {},
   "outputs": [],
   "source": [
    "veh.deriv(veh.q, [0.3, 0.2])"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
